#include <stdio.h>
#include <math.h>
#include "nijie.h"
#include "oslib.h"
#define pi 3.1415926535898
#define A1 0.0415
#define L1 0.03125
#define L2 0.09225
#define L3 0.13013 // 单腿的各个杆长尺寸，单位为m

void  simulate(double A[3])
{
    double B[4]; 
	//为存储舵机角度值的数组，B[0]为舵机1的角度，B[1] B[2]为舵机2角度的两个取值，B[3]为舵机3的角度
    B[0] = -atan(A[0] / A[1]);
    double C_lhl[3][3] = { { cos(B[0]),sin(B[0]),0 },{ -sin(B[0]), cos(B[0]), 0},{0, 0, 1} };//是求过逆之后的结果
    double M[3] = { 0,0,0 };
    int i, j;
    for (i = 0; i < 3; i++)
        for (j = 0; j < 3; j++)
            M[i] += A[j] * C_lhl[i][j];//进行矩阵乘法 M数组相当于MATLAB程序中的A
    M[1] = M[1] - L1;
    M[2] = M[2] - A1;
    B[3] = acos((M[1] * M[1] + M[2] * M[2] - L2 * L2 - L3 * L3) / (2 * L2 * L3));

    double a = M[1] + L3 * cos(B[3]) + L2;
    double b = 2 * L3 * sin(B[3]);
    double c = M[1] - L3 * cos(B[3]) - L2;
    double delta = b * b - 4 * a * c;
    double t1, t2;
    if (delta > 0)
    {
        t1 = (-b + sqrt(delta)) / (2 * a);
        t2 = (-b - sqrt(delta)) / (2 * a);
    }
    B[1] = 2 * atan(t1);
    B[2] = 2 * atan(t2);
    for (i = 0; i < 4; i++)
        uprintf("%lf ", B[i]);
}

// int main()
// {
//     double A[3];
//     int i;
//   // for (i = 0; i < 3; i++)
//      // scanf_s("%f", &A[i]);//手动输入目的足端坐标值（x,y,z）
//     A[0] = -0.1456;
//     A[1] = 0.1736;
//     A[2] = -0.0215;
//     simulate(A);//A为存储位置的数组

//     return 0;
// }
